import numpy as np

from parameters import *
from DStarLiteRouting import DStarLiteRouting

class MyRouting:
    def __init__(self, manager):
        self.map = np.zeros((map_size, map_size), dtype=np.bool8)
        self.start = (map_size // 2, map_size // 2)
        self.goal = (self.start[0] - 8, self.start[1] + 5) # assign goal manually

        self.dStarLite = DStarLiteRouting(self.map, self.start, self.goal)

        self.directions = np.asarray([[0, 1], [1, 0], [0, -1], [-1, 0]])
        self.direction = 0

        self.pubsub_manager = manager
        
        self.infrared_subscriber = self.pubsub_manager.create_subscription('infrared', self.infrared_callback)

        # reserve for another node to assign goal
        # self.goal_subscriber = self.pubsub_manager.create_subscription('goal', self.goal_callback)

        self.movement_publisher = self.pubsub_manager.create_publisher('movement')

    def infrared_callback(self, msg):
        print('node routing receives infrared distance', msg)
        self.update_map(msg)

    # def goal_callback(self, msg):
    #     self.goal = msg
    #     self.dStarLite.goal = msg

    def movement_callback(self, turning):
        # we assume that the movement is successful
        if turning == 0:
            self.start = (self.start[0] + self.directions[self.direction, 0], self.start[1] + self.directions[self.direction, 1])
            # check if the robot is still in the map?
        elif turning > 0 and turning < 4:
            self.direction = (self.direction + turning) % 4
        else:
            pass
            
        msg = turning
        self.movement_publisher.publish(msg)
        print('node routing publishes movement', msg)

    def update_map(self, distance):
        updates = []

        for i in range(1, distance):
            pos = (self.start[0] + i * self.directions[self.direction, 0], self.start[1] + i * self.directions[self.direction, 1])
            if pos[0] < 0 or pos[0] >= self.map.shape[0] or pos[1] < 0 or pos[1] >= self.map.shape[1]:
                break
            if self.map[pos]:
                updates.append(pos)
        pos = (self.start[0] + distance * self.directions[self.direction, 0], self.start[1] + distance * self.directions[self.direction, 1])
        if not (pos[0] < 0 or pos[0] >= self.map.shape[0] or pos[1] < 0 or pos[1] >= self.map.shape[1]):
            if not self.map[pos]:
                updates.append(pos)

        self.dStarLite.updateMap(updates)
        self.dStarLite.assignTurning(self.direction, self.movement_callback)
